#include "ros/ros.h"
#include "pure_pursuit_core.h"
#include"math.h"
#include <algorithm>
#include<limits>
#include <climits>


namespace pure_pursuit_pkg
{
  PurePursuit::PurePursuit(double purePursuitHz_local,bool linear_interpolate_mode) :
      speedPidCtr(0.5,0.5,0,1/purePursuitHz_local,-1,1,-1,1),
      positionPidCtr(0.5,0,0,1/purePursuitHz_local,0,30,0,30)
    {   
      deltaNumToStop = 10;
      hardBrakeDistance = 0.2;  
      purePursuitHz = purePursuitHz_local;
      linear_interpolate_ = linear_interpolate_mode;     
      
      num_of_next_waypoint_ = std::numeric_limits<int>::max();
      num_of_closet_waypoint_ = std::numeric_limits<int>::max();
      num_of_destination_waypoint_ = std::numeric_limits<int>::max();
      isFindclosetWaypoint = false;
      isFindNextWaypoint = false;  
      lookahead_distance_ = 6;
      displacement_threshold_ = lookahead_distance_;
      axleBase = 2.91;
      server.setCallback(boost::bind(&PurePursuit::parameterReconfigCB, this, _1,_2));
      speedCmd = 3.0;
      isDestinationArrive = false;
      /*monitor
      void setAllParameter(int _foodPerTurn=0, int _hungerPerTurn=0, int _hungerMin=0,\
          int _criticalPoint=50, int _hungerMax=100); */
      currentPoseMsgWatchDog.setAllParameter(8,1,0,3,8);
      twistMsgWatchDog.setAllParameter(8,1,0,3,8);
      wayPointsMsgWatchDog.setAllParameter(8,1,0,3,8);  
      destinationPointNumWatchDog.setAllParameter(8,1,0,3,8);
      positionPidCtr.setDeltaTime(1/purePursuitHz);
      speedPidCtr.setDeltaTime(1/purePursuitHz); 
      //ff = boost::bind(&PurePursuit::parameterReconfigCB,this, _1);
      
      //server.setCallback(ff);

      //mode control
      speed_mode = TORQUE_MODE;
      position_mode = NULL_MODE;
      steering_mode = AUTO_MODE;
      

      speedTestOnly = false;         //test pid speed ctr
      speedPositonTestOnly = false;  //test speed and postion ctr
      PositonTestOnly = false;       //test  postion ctr and speed ctr is not done by ros
      stopFlag = false;                   //hard brake or no drive if dont have brake system
      speedCmd = 0;                  //speed cmd under speedTestOnly mode
      outFile.open("data.csv", std::ios::ate|std::ios::out);
      outFile << "total_num" << ',' << "closet_num" << ',' << "next_num" <<','<< "closet.x"<<','
              <<"closet.y"<<','<<"next.x"<<','<<"next.y"<<std::endl;
    }
  void PurePursuit::parameterReconfigCB(pure_pursuit_pkg::purePursuitConfig &config,uint32_t level)//pid 参数配置回调函数
  {
    //speedPid赋值
    if(level==1)
    {
      speedPidCtr.Kp=config.speedKp;
      speedPidCtr.Ki=config.speedKi;
      speedPidCtr.Kd=config.speedKd;
      speedPidCtr.IntergrationDownLimit=config.speedIntergrationDownLimit;
      speedPidCtr.IntergrationUplimit=config.speedIntergrationUplimit;
      speedPidCtr.downLimit=config.speedDownLimit;
      speedPidCtr.upLimit=config.speedUpLimit;
      ROS_INFO("speed pid parameter is reconfigured: %f %f %f", 
          speedPidCtr.Kp,speedPidCtr.Ki,speedPidCtr.Kd);
      ROS_INFO(" %f %f %f %f",
          speedPidCtr.IntergrationDownLimit,speedPidCtr.IntergrationUplimit,
          speedPidCtr.downLimit,speedPidCtr.upLimit);     
    }
    if(level==2)
    {
      positionPidCtr.Kp=config.positionKp;
      positionPidCtr.Ki=config.positionKi;
      positionPidCtr.Kd=config.positionKd;
      positionPidCtr.IntergrationDownLimit=config.positionIntergrationDownLimit;
      positionPidCtr.IntergrationUplimit=config.positionIntergrationUplimit;
      positionPidCtr.downLimit=config.positionDownLimit;
      positionPidCtr.upLimit=config.positionUpLimit;
      speedPidCtr.Kp=config.speedKp;
      ROS_INFO("position parameter is reconfigured: %f %f %f",
          positionPidCtr.Kp,positionPidCtr.Ki,positionPidCtr.Kd);
      ROS_INFO(" %f %f %f %f",
          positionPidCtr.IntergrationDownLimit,positionPidCtr.IntergrationUplimit,
          positionPidCtr.downLimit,positionPidCtr.upLimit);  
    }
    if(level==3)
    {
      speedTestOnly=config.speedTestOnly;        //test pid speed ctr
      speedPositonTestOnly=config.speedPositonTestOnly;  //test speed and postion ctr
      PositonTestOnly=config.PositonTestOnly;       //test  postion ctr and speed ctr is not done by ros
      stop=config.stop;                  //hard brake or no drive if dont have brake system
      speedCmd=config.speedCmd;                 //speed cmd under speedTestOnly mode
      ROS_INFO(" %d %d %d %d",speedTestOnly,speedPositonTestOnly,PositonTestOnly,stop); 
    }  
    if(level==4)
    {
      axleBase=config.axleBase;
      lookahead_distance_=config.lookahead_distance;
      ROS_INFO("axleBase and lookahead_distance is reconfigiured %f %f",
              axleBase,lookahead_distance_);

    }
      
    
  
      ROS_INFO("level is %d",level);

  }
  void  PurePursuit::callbackFromCurrentPose(const geometry_msgs::PoseStampedConstPtr &msg)
  {
    current_pose_.header = msg->header;
    current_pose_.pose = msg->pose;
    currentPoseMsgWatchDog.giveFood();  
    ROS_INFO("current pose is %f,%f",msg->pose.position.x,msg->pose.position.y);
  }//end function
  void PurePursuit::callbackFromCurrentVelocity(const geometry_msgs::TwistStampedConstPtr &msg)
  {
    //ROS_INFO("in call back %f",current_velocity_.twist.linear.x);
    current_velocity_ = *msg;
    
    twistMsgWatchDog.giveFood(); 
  }//end function

  void PurePursuit::callbackFromWayPoints(const styx_msgs::LaneConstPtr &msg)
  {
    //ROS_INFO("final_waypoints is arrived in pure_pursuit");
    current_waypoints_.setPath(*msg);
    wayPointsMsgWatchDog.giveFood(); 
  }//end funtcion
  void PurePursuit::callbackDestinationPointNum(const std_msgs::Int32ConstPtr &msg)
  {
    num_of_destination_waypoint_=msg->data;
    destinationPointNumWatchDog.giveFood();
  }//end function

  double PurePursuit::getCmdVelocity(int waypoint) 
  {
    if (current_waypoints_.isEmpty())
    {
      ROS_INFO_STREAM("waypoint : not loaded path");
      return 0;
    }

    double velocity = current_waypoints_.getWaypointVelocityMPS(waypoint);
    // ROS_INFO_STREAM("waypoint : " << mps2kmph(velocity) << " km/h ( " << velocity << "m/s )");
    return velocity;
  }//end function

  double PurePursuit::calcCurvature(geometry_msgs::Point target) 
  {
    double kappa=0;
    double denominator = pow(getPlaneDistance(target, current_pose_.pose.position), 2);  
    double numerator = calcRelativeCoordinate(target, current_pose_.pose).y; 
    if(denominator==0)
    {
      ROS_INFO("target point can not be same as robot current point");
      return 0;
    }  
    
    kappa = 2 *numerator/ denominator;
    ROS_INFO("RelativeCoordinate is %f %f",calcRelativeCoordinate(target, current_pose_.pose).x,
              calcRelativeCoordinate(target, current_pose_.pose).y);
    ROS_INFO("num is %f ,den is %f,kappa is %f",numerator,denominator,kappa);  
    return kappa;
  }//end function

  // linear interpolation of next target
  bool PurePursuit::interpolateNextTarget(int next_waypoint, geometry_msgs::Point *next_target) 
  {
    
    constexpr double ERROR = pow(10, -5);  // 0.00001   
    if (!isFindNextWaypoint)
    {
        ROS_ERROR("interpolateNextTarget failed due to nextWaypoint is not found");
        return false;       
    }
    double search_radius = lookahead_distance_;
    geometry_msgs::Point zero_p;
    geometry_msgs::Point end = current_waypoints_.getWaypointPosition(next_waypoint);
    geometry_msgs::Point start = current_waypoints_.getWaypointPosition(next_waypoint - 1);

    // let the linear equation be "ax + by + c = 0"
    // if there are two points (x1,y1) , (x2,y2), a = "y2-y1, b = "(-1) * x2 - x1" ,c = "(-1) * (y2-y1)x1 + (x2-x1)y1"
    double a = 0;
    double b = 0;
    double c = 0;
    double get_linear_flag = getLinearEquation(start, end, &a, &b, &c);
    if (!get_linear_flag)
    {
      ROS_ERROR("interpolateNextTarget failed due to getLinearEquation");
      return false;
    }
      
    
    // let the center of circle be "(x0,y0)", in my code , the center of circle is vehicle position
    // the distance  "d" between the foot of a perpendicular line and the center of circle is ...
    //    | a * x0 + b * y0 + c |
    // d = -------------------------------
    //          √( a~2 + b~2)
    double d = fabs(getDistanceBetweenLineAndPoint(current_pose_.pose.position, a, b, c));


    //**************************************************************************
    // unit vector of point 'start' to point 'end'
    tf::Vector3 v((end.x - start.x), (end.y - start.y), 0);
    tf::Vector3 unit_v = v.normalize();
    // normal unit vectors of v
    tf::Vector3 unit_w1 = rotateUnitVector(unit_v, 90);   // rotate to counter clockwise 90 degree
    tf::Vector3 unit_w2 = rotateUnitVector(unit_v, -90);  // rotate to counter clockwise 90 degree
    // the foot of a perpendicular line
    geometry_msgs::Point h1;
    h1.x = current_pose_.pose.position.x + d * unit_w1.getX();
    h1.y = current_pose_.pose.position.y + d * unit_w1.getY();
    h1.z = current_pose_.pose.position.z;

    geometry_msgs::Point h2;
    h2.x = current_pose_.pose.position.x + d * unit_w2.getX();
    h2.y = current_pose_.pose.position.y + d * unit_w2.getY();
    h2.z = current_pose_.pose.position.z;

    geometry_msgs::Point h;
    if (fabs(a * h1.x + b * h1.y + c) < ERROR)
    {
      h = h1;
      //   ROS_INFO("use h1");
    }
    else if (fabs(a * h2.x + b * h2.y + c) < ERROR)
    {
      //   ROS_INFO("use h2");
      h = h2;
    }
    else
    {
      ROS_ERROR("interpolateNextTarget failed due to foot of a perpendicular line failed");
      return false;
    }

    // get intersection[s]
    // if there is a intersection
    if (d == search_radius)
    {
      *next_target = h;
      return true;
    }
    else
    {
      // if there are two intersection
      // get intersection in front of vehicle
      double s = sqrt(pow(search_radius, 2) - pow(d, 2));
      geometry_msgs::Point target1;
      target1.x = h.x + s * unit_v.getX();
      target1.y = h.y + s * unit_v.getY();
      target1.z = current_pose_.pose.position.z;

      geometry_msgs::Point target2;
      target2.x = h.x - s * unit_v.getX();
      target2.y = h.y - s * unit_v.getY();
      target2.z = current_pose_.pose.position.z;
      
      // check intersection is between end and start
      double interval = getPlaneDistance(end, start);
      if (getPlaneDistance(target1, end) < interval)
      {
        // ROS_INFO("result : target1");
        *next_target = target1;
        ROS_INFO("target is bettwen start and end");
        return true;
      }
      else if (getPlaneDistance(target2, end) < interval)
      {
        // ROS_INFO("result : target2");
        *next_target = target2;
        ROS_WARN("target is bettwen start and end");
        return true;
      }
      else
      {      
        double tempSignTarget1x = (target1.x-end.x)*(start.x-end.x);
        double tempSignTarget1y = (target1.y-end.y)*(start.y-end.y);
        double tempSignTarget2x = (target2.x-end.x)*(start.x-end.x);
        double tempSignTarget2y = (target2.y-end.y)*(start.y-end.y);
        if( (tempSignTarget1x>0) || (tempSignTarget1y>0) )
        {
          *next_target = target1;
           ROS_WARN("target is before end");
           return true;
        }
        else if((tempSignTarget2x>0) || (tempSignTarget2y>0))
        {
          *next_target = target2;
           ROS_WARN("target is before end");
           return true;
        }    
        else
        {           
          ROS_ERROR("interpolateNextTarget failed due to unkonwn reseaon");
          return false;
        }     
      }     
  
  }
  }//end funtion

  bool PurePursuit::verifyFollowing() 
  {
    double a = 0;
    double b = 0;
    double c = 0;
    bool Flag = true;
    latral_displacement_ = std::numeric_limits<double>::max(); 
    int path_size = current_waypoints_.getSize();
    if(num_of_closet_waypoint_ == 0)
    {
      Flag &= getLinearEquation(current_waypoints_.getWaypointPosition(0),
              current_waypoints_.getWaypointPosition(1), &a, &b, &c);
      if(Flag)
      {
        latral_displacement_  = fabs(getDistanceBetweenLineAndPoint(current_pose_.pose.position, a, b, c));
      }
    }
    else if( num_of_closet_waypoint_ == (path_size-1) )
    {
      Flag &= getLinearEquation(current_waypoints_.getWaypointPosition(num_of_closet_waypoint_),
              current_waypoints_.getWaypointPosition(num_of_closet_waypoint_-1), &a, &b, &c);
      if(Flag)
      {
        latral_displacement_  = fabs(getDistanceBetweenLineAndPoint(current_pose_.pose.position, a, b, c));
      }     
    }
    else
    {
      bool Flag1= true;
      bool Flag2= true;
      double displacement_1 = std::numeric_limits<double>::max(); 
      Flag1 &= getLinearEquation(current_waypoints_.getWaypointPosition(num_of_closet_waypoint_), 
              current_waypoints_.getWaypointPosition(num_of_closet_waypoint_-1), &a, &b, &c);
      if(Flag1)
      {
        latral_displacement_  = fabs(getDistanceBetweenLineAndPoint(current_pose_.pose.position, a, b, c));
      }
      Flag2 &= getLinearEquation(current_waypoints_.getWaypointPosition(num_of_closet_waypoint_),
               current_waypoints_.getWaypointPosition(num_of_closet_waypoint_+1), &a, &b, &c);
      if(Flag2)
      {
        displacement_1 = fabs(getDistanceBetweenLineAndPoint(current_pose_.pose.position, a, b, c));
      }
      latral_displacement_ =std::min(latral_displacement_ ,displacement_1);
      Flag = Flag1 && Flag2; 
    }
    if(Flag==false)
    {
      ROS_ERROR("verifyFollowing failed due to getLinear fun failed");
      return false;
    }
    ROS_INFO("lateral dispacement is %f",latral_displacement_ );
    if (fabs(latral_displacement_  < displacement_threshold_) )
    {
      ROS_INFO("Following : True");      
      return true;
    }
    else
    {
      ROS_INFO("Following : False");
      return false;
    }
  }//end function

  bool PurePursuit::searchNextAndClosetPoint()
  {    
    isFindclosetWaypoint=false;
    isFindNextWaypoint=false;
    num_of_closet_waypoint_=std::numeric_limits<int>::max();
    num_of_next_waypoint_=std::numeric_limits<int>::max();
    double distance=std::numeric_limits<double>::max();
    double nextDistance=std::numeric_limits<double>::max();
    double lastDistance=std::numeric_limits<double>::max();
    int path_size = static_cast<int>(current_waypoints_.getSize());
    // if waypoints are not given or to loog no singnal reacive, do nothing.
    if (path_size <3)
    {
      //num_of_next_waypoint_ = -1;
      ROS_ERROR("searchNextAndClosetPoint failed due to path size short than 3");     
      return false;
    }

    // look for the next waypoint.
    for (int i = 0; i < path_size; i++)
    {
      // if search waypoint is the last
     if (i == (path_size - 1))
      {
        double tempDistance = getPlaneDistance(current_waypoints_.getWaypointPosition(i)
                          , current_pose_.pose.position);
        ROS_WARN("last point distance is %f ",tempDistance);
        if( isFindclosetWaypoint && (isFindNextWaypoint==false) )
        {
          ROS_WARN("closet point is found,but not next point ");
          num_of_next_waypoint_=i;
          isFindNextWaypoint=true;
          return true;
        }

        else if( tempDistance<lookahead_distance_ && (isFindNextWaypoint==false) )
        {
          ROS_WARN("closet point and next point is last point ");
          num_of_closet_waypoint_ = i;
          isFindclosetWaypoint=true;
          num_of_next_waypoint_=i;
          isFindNextWaypoint=true;
          return true;
        }
        else
        {
          ROS_ERROR("searchNextAndClosetPoint failed due to find to last point failed"); 
          return false;
        }
                
      }

      // if there exists an effective waypoint
      nextDistance=getPlaneDistance(current_waypoints_.getWaypointPosition(i+1), current_pose_.pose.position);
         
      if ( isFindNextWaypoint==false &&(lastDistance<lookahead_distance_)\
          && (distance>lookahead_distance_) )
      {
        ROS_INFO("preview point num and distance is %d %f",i,distance);   
        num_of_next_waypoint_ = i;
        isFindNextWaypoint=true;
        //ROS_ERROR_STREAM("wp = " << i << " dist = " << getPlaneDistance(current_waypoints_.getWaypointPosition(i), current_pose_.pose.position) );
      }
      if ( isFindclosetWaypoint==false && (lastDistance>distance)
          && (nextDistance>distance) && distance<lookahead_distance_ )
      {
        ROS_WARN("closet point num and distance is %d %f",i,distance); 
        num_of_closet_waypoint_ = i;
        isFindclosetWaypoint=true;
        //ROS_ERROR_STREAM("wp = " << i << " dist = " << getPlaneDistance(current_waypoints_.getWaypointPosition(i), current_pose_.pose.position) );
      }
      lastDistance=distance;
      distance=nextDistance; 
      if(isFindNextWaypoint && isFindclosetWaypoint) 
      {
        return true;
      }
    }

    if(!isFindNextWaypoint)
    {      
      ROS_ERROR("searchNextAndClosetPoint failed due to next waypoint is not found");     
      return false;
    }
    if(!isFindclosetWaypoint)
    {
      ROS_ERROR("searchNextAndClosetPoint failed due to closet waypoint is not found");     
      return false;    
    } 
    ROS_ERROR("searchNextAndClosetPoint failed due to unkown reasean");     
    return false;
  }//end function

  bool PurePursuit::lateralCtr()
  {
  
    // search next waypoint
    bool interpolate_flag = false;
    //calcLookaheadDistance();
    ROS_INFO("lookahead distance: %f\n", lookahead_distance_);
    if (!linear_interpolate_ || num_of_next_waypoint_ == 0)
    {
      position_of_next_target_ = current_waypoints_.getWaypointPosition(num_of_next_waypoint_);
      tireAngleDeg=atan( calcCurvature(position_of_next_target_)*axleBase );
      ROS_INFO("tire angle is %f",tireAngleDeg);
      return true;
    }
    // linear interpolation and calculate angular velocity
    interpolate_flag = interpolateNextTarget(num_of_next_waypoint_, &position_of_next_target_);  
    if (!interpolate_flag)
    {  
      ROS_ERROR("lateral ctr error due to interpolateNextTarget failed");
      return false;
    }  
    tireAngleDeg=atan( calcCurvature(position_of_next_target_)*axleBase );
    ROS_INFO("tire angle is %f",tireAngleDeg);
    return true;
  }//end function
  bool PurePursuit::longtitudeCtr(double& tempAcc)
  { 
    tempAcc=-1; 
    double speedError= 0;
    //if(speedTestOnly) 
    if(0)
    {
      speedError=speedCmd-current_velocity_.twist.linear.x;
    }   
    else if(position_mode == NULL_MODE)
    {          
        speedError = current_waypoints_.getWaypointVelocityMPS(num_of_closet_waypoint_)
                    -current_velocity_.twist.linear.x;
    }
    else //position_mode == POSITION_MODE
    {
      double tempSpeed = 0;
      if( positionCtr(tempSpeed) )// bool PurePursuit::positionCtr(double& tempSpeedCmd)
      {
        speedError = tempSpeed-current_velocity_.twist.linear.x;
      }
      else
      {
        ROS_ERROR("longtitudeCtr failed due to positionCtr failed");
        tempAcc = -1;
        return false;
      }    
    }//end else   

    ROS_INFO("speed cmd of longtitude control is %f",speedCmd);
    ROS_INFO("speed Error is %f",speedError);
    tempAcc = speedPidCtr.step(speedError);
    ROS_INFO("speed ctr intergration is %f,speed ctr output is %f",
            speedPidCtr.Intergration,speedPidCtr.output);
    ROS_INFO("current speed is %f",current_velocity_.twist.linear.x);
    return true;
  }//end function
  bool PurePursuit::positionCtr(double& tempSpeedCmd)
  {    
    tempSpeedCmd=0;
    double positionError;
    if( num_of_destination_waypoint_> (current_waypoints_.getSize()-1) )
    {
      ROS_WARN("num_of_destination_waypoint_ is outside of waypoints");
      positionError = std::numeric_limits<double>::max();
    }
    else
    {
      positionError=getPlaneDistance( current_pose_.pose.position, 
          current_waypoints_.getWaypointPosition(num_of_destination_waypoint_) );
    }    
   
    ROS_INFO("distance to destination is %f num of destination is %d",positionError,
            num_of_destination_waypoint_); 


    int deltaNumPoint = num_of_destination_waypoint_ - num_of_closet_waypoint_;
    if( deltaNumPoint>deltaNumToStop )
    {
      isDestinationArrive = false;
      tempSpeedCmd=current_waypoints_.getWaypointVelocityMPS(num_of_closet_waypoint_);
      return true;      
    } 
    else if(deltaNumPoint<0)
    {
      ROS_WARN("positionCtr failed due to closet_waypoint_ is bigger than destination_waypoint_");
      tempSpeedCmd=0;
      return false;
    }    
    else
    {
      double tempDistanceToClosetPoint ;
      double tempDistanceToStop ;
      if (deltaNumPoint==0)
      {
        tempDistanceToClosetPoint = getPlaneDistance(
                            current_waypoints_.getWaypointPosition(num_of_closet_waypoint_),
                            current_pose_.pose.position);
        tempDistanceToStop = tempDistanceToClosetPoint;        
      }  
      else
      {
        tempDistanceToClosetPoint = getPlaneDistance(
                            current_waypoints_.getWaypointPosition(num_of_closet_waypoint_+1),
                            current_pose_.pose.position);
        tempDistanceToStop = tempDistanceToClosetPoint;
        for(int i = num_of_closet_waypoint_+1;i<num_of_destination_waypoint_;i++)
        {        
          tempDistanceToStop+=getPlaneDistance(current_waypoints_.getWaypointPosition(i),
                              current_waypoints_.getWaypointPosition(i+1));                                                      
        }      
      }
      if( isDestinationArrive )
      {
        ROS_ERROR("positionCtr failed due to destinationArrive ,should be ignorded");
        return false;
      }   
       
      double tempAcc=pow(current_velocity_.twist.linear.x,2)/2/tempDistanceToStop;
      tempSpeedCmd = pow( std::max( pow(current_velocity_.twist.linear.x,2)-
                        2*tempDistanceToClosetPoint*tempAcc,0.02 ) , 0.5);   
      ROS_INFO("tempDistanceToStop is %f tempSpeedCmd is %f",tempDistanceToStop,tempSpeedCmd);  
      if( tempDistanceToStop< hardBrakeDistance )
      {      
        isDestinationArrive =true;
        tempSpeedCmd=0;        
        ROS_ERROR("in hardBrakeMode,logtitude ctr error can be ignored");
        return false;
      }           
      return true;
    }

  }
  bool PurePursuit::checkDog()
  {
    currentPoseMsgWatchDog.noFood();  
    twistMsgWatchDog.noFood();
    wayPointsMsgWatchDog.noFood();
    //destinationPointNumWatchDog.noFood();
    bool flag = true;
    if( currentPoseMsgWatchDog.isDied() )
    {
      ROS_ERROR("currentPoseMsgWatchDog time out");
      flag = false;
    }
    if( twistMsgWatchDog.isDied() )
    {
      ROS_ERROR("twistMsgWatchDog time out");
      flag = false;
    }
    if( wayPointsMsgWatchDog.isDied() )
    {
      ROS_ERROR("wayPointsMsgWatchDog time out");
      flag = false;
    }
    /*if( destinationPointNumWatchDog.isDied() )
    {
      ROS_ERROR("destinationPointNumWatchDog time out");
      flag = false;
    }*/
    return flag;
  }
  void PurePursuit::hard_brake()
  {
      norAcc=0;
      norBrake=1;
  }
  bool PurePursuit::step()
  {
    if( checkDog() == false )
    {
      hard_brake();  
      return false;
    }
    update_lookahead_distance();
    if( searchNextAndClosetPoint() == false )
    {
      hard_brake();  
      return false;
    }  
    print_to_csv();
    if( verifyFollowing() == false)
    {
      hard_brake();
      return false;
    } 

    if( lateralCtr() == false)
    {
      hard_brake();
      return false;
    } 
    double tempAcc=-1;
    if( longtitudeCtr(tempAcc) == false)
    {
      hard_brake();
      return false;
    }  
    if( tempAcc>=0 )      
    {
      norAcc=tempAcc;
      norBrake=0;
    }
    else
    {
      norBrake=-tempAcc;
      norAcc=0;
    }  
    return true;
  } //end function
  void PurePursuit::print_to_csv()
  {
    /*outFile << "total_num" << ',' << "closet_num" << ',' << "next_num" <<','<< "closet.x"<<','
              <<"closet.y"<<','<<"next.x"<<','<<"next.y"<<endl;*/
    outFile << static_cast<int>(current_waypoints_.getSize()) << ',' 
    << num_of_closet_waypoint_ << ',' << num_of_next_waypoint_ <<','
    << current_waypoints_.getWaypointPosition(num_of_closet_waypoint_).x<<','
    << current_waypoints_.getWaypointPosition(num_of_closet_waypoint_).y<<','
    << current_waypoints_.getWaypointPosition(num_of_next_waypoint_).x<<','
    << current_waypoints_.getWaypointPosition(num_of_next_waypoint_).y<<std::endl;
  }
  void PurePursuit::CBLocalizition(const auto_msgs::localizition &msg)
  {
    current_pose_.pose.position.x= msg.x;
    current_pose_.pose.position.y= msg.y;
    current_pose_.pose.orientation.x=msg.qx;
    current_pose_.pose.orientation.y=msg.qy;
    current_pose_.pose.orientation.z=msg.qz;
    current_pose_.pose.orientation.w=msg.qw;
    ROS_INFO("current pose is %f,%f",current_pose_.pose.position.x,current_pose_.pose.position.y);
    currentPoseMsgWatchDog.giveFood();  
  }
  void PurePursuit::CBChassis(const auto_msgs::chassis &msg)
  {
    current_velocity_.twist.linear.x = msg.speed_mps;    
    twistMsgWatchDog.giveFood();
  }
  void PurePursuit::CBPlanning(const auto_msgs::planning &msg)
  {
    styx_msgs::Lane temp_lane;
    styx_msgs::Waypoint temp_waypoint;    
    for(auto iter = msg.path.begin(); iter != msg.path.end(); iter++)
    {
      temp_waypoint.pose.pose.position.x=iter->x;
      temp_waypoint.pose.pose.position.y=iter->y;
      temp_waypoint.pose.pose.position.z=0;
      temp_waypoint.twist.twist.linear.x=iter->v/3.6*0.8;
      temp_lane.waypoints.push_back(temp_waypoint);    }
      current_waypoints_.setPath(temp_lane);
      wayPointsMsgWatchDog.giveFood(); 
  }
  void PurePursuit::update_lookahead_distance()
  {
    lookahead_distance_=current_velocity_.twist.linear.x*lookahead_distance_ratio_;
    if(lookahead_distance_<min_lookahead_distance_)
    {
      lookahead_distance_ = min_lookahead_distance_;
    }
    else if(lookahead_distance_>max_lookahead_distance_)
    {
      lookahead_distance_=max_lookahead_distance_;
    }
    else
    {

    }
  }
}//end namespace

